/**********************************************************************************************************************
 *
 * Project:    mGameEngine
 * File:       matrix4x4.h
 * Author:     karooolek
 * Created on: 2009-02-26
 *
 **********************************************************************************************************************/

#ifndef MATRIX4X4_H_
#define MATRIX4X4_H_

#include "vector4.h"
#include "quaternion.h"
#include <cstring>
#include <algorithm>
#include <cstdio>

namespace mGameEngine
{

/**
 * Four by four floats matrix.
 */
struct Matrix4x4
{
    /**
     * Matrix components
     */
    union
    {
        float m[4][4];
        float _m[16];
    };

    /**
     * Create matrix with initial value on diagonal.
     * @param a diagonal value.
     */
    Matrix4x4(float a = 1.0f)
    {
        _m[0] = a;    _m[4] = 0.0f; _m[8] = 0.0f;  _m[12] = 0.0f;
        _m[1] = 0.0f; _m[5] = a;    _m[9] = 0.0f;  _m[13] = 0.0f;
        _m[2] = 0.0f; _m[6] = 0.0f; _m[10] = a;    _m[14] = 0.0f;
        _m[3] = 0.0f; _m[7] = 0.0f; _m[11] = 0.0f; _m[15] = a;
    }

    /**
     * Create matrix with initial values.
     * @param m00 first column, first row
     * @param m10 second column, first row
     * @param m20 third column, first row
     * @param m30 fourth column, first row
     * @param m01 first column, second row
     * @param m11 second column, second row
     * @param m21 third column, second row
     * @param m31 fourth column, second row
     * @param m02 first column, third row
     * @param m12 second column, third row
     * @param m22 third column, third row
     * @param m32 fourth column, third row
     * @param m03 first column, fourth row
     * @param m13 second column, fourth row
     * @param m23 thrid column, fourth row
     * @param m33 fourth column, fourth row
     */
    Matrix4x4(float m00, float m10, float m20, float m30,
              float m01, float m11, float m21, float m31,
              float m02, float m12, float m22, float m32,
              float m03, float m13, float m23, float m33)
    {
        _m[0] = m00; _m[4] = m10; _m[8] = m20;  _m[12] = m30;
        _m[1] = m01; _m[5] = m11; _m[9] = m21;  _m[13] = m31;
        _m[2] = m02; _m[6] = m12; _m[10] = m22; _m[14] = m32;
        _m[3] = m03; _m[7] = m13; _m[11] = m23; _m[15] = m33;
    }

    /**
     * Create matrix with initial values from four vectors.
     * @param v0 first column
     * @param v1 second column
     * @param v2 third column
     * @param v3 fourth column
     */
    Matrix4x4(const Vector4 &v0,
              const Vector4 &v1,
              const Vector4 &v2,
              const Vector4 &v3)
    {
        _m[0] = v0.x; _m[4] = v1.x; _m[8] = v2.x;  _m[12] = v3.x;
        _m[1] = v0.y; _m[5] = v1.y; _m[9] = v2.y;  _m[13] = v3.y;
        _m[2] = v0.z; _m[6] = v1.z; _m[10] = v2.z; _m[14] = v3.z;
        _m[3] = v0.w; _m[7] = v1.w; _m[11] = v2.w; _m[15] = v3.w;
    }

    /**
     * Create matrix from 2x2 sub-matrix.
     * @param m2 sub-matrix to copy.
     */
    Matrix4x4(const Matrix2x2 &m2)
    {
        operator=(m2);
    }

    /**
     * Create matrix from 3x3 sub-matrix.
     * @param m2 sub-matrix to copy.
     */
    Matrix4x4(const Matrix3x3 &m2)
    {
        operator=(m2);
    }

    /**
     * Copy constructor.
     * @param m2 matrix to copy.
     */
    Matrix4x4(const Matrix4x4 &m2)
    {
        operator=(m2);
    }


    /**
     * Create matrix with initial values from values array.
     * @param v sixteen floats array
     */
    Matrix4x4(const float *v)
    {
        memcpy(_m, v, 16 * sizeof(float));
    }

    /**
     * Create matrix with initial orientation.
     * @param q orientation quaternion.
     */
    Matrix4x4(const Quaternion &q)
    {
        float xx(q.x * q.x), yy(q.y * q.y), zz(q.z * q.z);
        float xy(q.x * q.y), xz(q.x * q.z), yz(q.y * q.z), wx(q.w*q.x), wy(q.w*q.y), wz(q.w*q.z);

        _m[0] = 1.0f - 2.0f * (yy + zz);
        _m[1] = 2.0f * (xy + wz);
        _m[2] = 2.0f * (xz - wy);
        _m[3] = 0.0f;

        _m[4] = 2.0f * (xy - wz);
        _m[5] = 1.0f - 2.0f * (xx + zz);
        _m[6] = 2.0f * (yz + wx);
        _m[7] = 0.0f;

        _m[8] = 2.0f * (xz + wy);
        _m[9] = 2.0f * (yz - wx);
        _m[10] = 1.0f - 2.0f * (xx + yy);
        _m[11] = 0.0f;

        _m[12] = 0.0f;
        _m[13] = 0.0f;
        _m[14] = 0.0f;
        _m[15] = 1.0f;
    }
    
    /**
     * Create matrix with initial translation.
     * @param v translation vector.
     */
    Matrix4x4(const Vector3 &v)
    {
        _m[0] = 1.0f; _m[4] = 0.0f; _m[8] = 0.0f;  _m[12] = v.x;
        _m[1] = 0.0f; _m[5] = 1.0f; _m[9] = 0.0f;  _m[13] = v.y;
        _m[2] = 0.0f; _m[6] = 0.0f; _m[10] = 1.0f; _m[14] = v.z;
        _m[3] = 0.0f; _m[7] = 0.0f; _m[11] = 0.0f; _m[15] = 1.0f;
    }
    /**
     * Create matrix with initial orientation and translation.
     * @param q orientation quaternion.
     * @param v translation vector.
     */
    Matrix4x4(const Quaternion &q, const Vector3 &v)
    {
        float xx(q.x * q.x), yy(q.y * q.y), zz(q.z * q.z);
        float xy(q.x * q.y), xz(q.x * q.z), yz(q.y * q.z), wx(q.w*q.x), wy(q.w*q.y), wz(q.w*q.z);

        _m[0] = 1.0f - 2.0f * (yy + zz);
        _m[1] = 2.0f * (xy + wz);
        _m[2] = 2.0f * (xz - wy);
        _m[3] = 0.0f;

        _m[4] = 2.0f * (xy - wz);
        _m[5] = 1.0f - 2.0f * (xx + zz);
        _m[6] = 2.0f * (yz + wx);
        _m[7] = 0.0f;

        _m[8] = 2.0f * (xz + wy);
        _m[9] = 2.0f * (yz - wx);
        _m[10] = 1.0f - 2.0f * (xx + yy);
        _m[11] = 0.0f;

        _m[12] = v.x;
        _m[13] = v.y;
        _m[14] = v.z;
        _m[15] = 1.0f;
    }

    /**
     * Assign 2x2 sub-matrix.
     * @param m2 matrix to assign.
     * @return matrix with assigned 2x2 values. Rest remains unchanged.
     */
    Matrix4x4 &operator=(const Matrix2x2 &m2)
    {
        _m[0] = m2._m[0]; _m[4] = m2._m[2];
        _m[1] = m2._m[1]; _m[5] = m2._m[3];
        return *this;
    }

    /**
     * Assign 3x3 sub-matrix.
     * @param m2 matrix to assign.
     * @return matrix with assigned 3x3 values. Rest remains unchanged.
     */
    Matrix4x4 &operator=(const Matrix3x3 &m2)
    {
        _m[0] = m2._m[0]; _m[4] = m2._m[3]; _m[8] = m2._m[6];
        _m[1] = m2._m[1]; _m[5] = m2._m[4]; _m[9] = m2._m[7];
        _m[2] = m2._m[2]; _m[6] = m2._m[5]; _m[10] = m2._m[8];
        return *this;
    }

    /**
     * Assign matrix.
     * @param m2 matrix to assign.
     * @return matrix with assigned values.
     */
    Matrix4x4 &operator=(const Matrix4x4 &m2)
    {
        memcpy(_m, m2._m, 16 * sizeof(float));
        return *this;
    }

    /**
     * Add matrix.
     * @param m2 matrix to add
     * @return sum of matrices
     */
    Matrix4x4 &operator+=(const Matrix4x4 &m2)
    {
        _m[0] += m2._m[0]; _m[4] += m2._m[4]; _m[8] += m2._m[8]; _m[12] += m2._m[12];
        _m[1] += m2._m[1]; _m[5] += m2._m[5]; _m[9] += m2._m[9]; _m[13] += m2._m[13];
        _m[2] += m2._m[2]; _m[6] += m2._m[6]; _m[10] += m2._m[10]; _m[14] += m2._m[14];
        _m[3] += m2._m[3]; _m[7] += m2._m[7]; _m[11] += m2._m[11]; _m[15] += m2._m[15];
        return *this;
    }

    /**
     * Add matrices.
     * @param m2 matrix to add
     * @return sum of matrices
     */
    Matrix4x4 operator+(const Matrix4x4 &m2) const
    {
        return Matrix4x4(
            _m[0] + m2._m[0], _m[4] + m2._m[4], _m[8] + m2._m[8], _m[12] + m2._m[12],
            _m[1] + m2._m[1], _m[5] + m2._m[5], _m[9] + m2._m[9], _m[13] + m2._m[13],
            _m[2] + m2._m[2], _m[6] + m2._m[6], _m[10] + m2._m[10], _m[14] + m2._m[14],
            _m[3] + m2._m[3], _m[7] + m2._m[7], _m[11] + m2._m[11], _m[15] + m2._m[15]);
    }

    /**
     * Negate matrix.
     * @return negated matrix
     */
    Matrix4x4 operator-() const
    {
        return *this * -1.0f;
    }

    /**
     * Subtract matrix.
     * @param m2 matrix to subtract
     * @return difference of matrices
     */
    Matrix4x4 &operator-=(const Matrix4x4 &m2)
    {
        _m[0] -= m2._m[0]; _m[4] -= m2._m[4]; _m[8] -= m2._m[8]; _m[12] -= m2._m[12];
        _m[1] -= m2._m[1]; _m[5] -= m2._m[5]; _m[9] -= m2._m[9]; _m[13] -= m2._m[13];
        _m[2] -= m2._m[2]; _m[6] -= m2._m[6]; _m[10] -= m2._m[10]; _m[14] -= m2._m[14];
        _m[3] -= m2._m[3]; _m[7] -= m2._m[7]; _m[11] -= m2._m[11]; _m[15] -= m2._m[15];
        return *this;
    }

    /**
     * Subtract matrices.
     * @param m2 matrix to subtract
     * @return difference of matrices
     */
    Matrix4x4 operator-(const Matrix4x4 &m2) const
    {
        return Matrix4x4(
            _m[0] - m2._m[0], _m[4] - m2._m[4], _m[8] - m2._m[8], _m[12] - m2._m[12],
            _m[1] - m2._m[1], _m[5] - m2._m[5], _m[9] - m2._m[9], _m[13] - m2._m[13],
            _m[2] - m2._m[2], _m[6] - m2._m[6], _m[10] - m2._m[10], _m[14] - m2._m[14],
            _m[3] - m2._m[3], _m[7] - m2._m[7], _m[11] - m2._m[11], _m[15] - m2._m[15]);
    }

    /**
     * Multiply matrix.
     * @param m2 matrix multilplier
     * @return multiplied matrix
     */
    Matrix4x4 &operator*=(const Matrix4x4 &m2)
    {
        float m00 = _m[0], m10 = _m[4], m20 = _m[8];
        float m01 = _m[1], m11 = _m[5], m21 = _m[9];
        float m02 = _m[2], m12 = _m[6], m22 = _m[10];
        float m03 = _m[3], m13 = _m[7], m23 = _m[11];

        _m[0] = m00 * m2._m[0] + m10 * m2._m[1] + m20 * m2._m[2] + _m[12] * m2._m[3];
        _m[4] = m00 * m2._m[4] + m10 * m2._m[5] + m20 * m2._m[6] + _m[12] * m2._m[7];
        _m[8] = m00 * m2._m[8] + m10 * m2._m[9] + m20 * m2._m[10] + _m[12] * m2._m[11];
        _m[12] = m00 * m2._m[12] + m10 * m2._m[13] + m20 * m2._m[14] + _m[12] * m2._m[15];

        _m[1] = m01 * m2._m[0] + m11 * m2._m[1] + m21 * m2._m[2] + _m[13] * m2._m[3];
        _m[5] = m01 * m2._m[4] + m11 * m2._m[5] + m21 * m2._m[6] + _m[13] * m2._m[7];
        _m[9] = m01 * m2._m[8] + m11 * m2._m[9] + m21 * m2._m[10] + _m[13] * m2._m[11];
        _m[13] = m01 * m2._m[12] + m11 * m2._m[13] + m21 * m2._m[14] + _m[13] * m2._m[15];

        _m[2] = m02 * m2._m[0] + m12 * m2._m[1] + m22 * m2._m[2] + _m[14] * m2._m[3];
        _m[6] = m02 * m2._m[4] + m12 * m2._m[5] + m22 * m2._m[6] + _m[14] * m2._m[7];
        _m[10] = m02 * m2._m[8] + m12 * m2._m[9] + m22 * m2._m[10] + _m[14] * m2._m[11];
        _m[14] = m02 * m2._m[12] + m12 * m2._m[13] + m22 * m2._m[14] + _m[14] * m2._m[15];

        _m[3] = m03 * m2._m[0] + m13 * m2._m[1] + m23 * m2._m[2] + _m[15] * m2._m[3];
        _m[7] = m03 * m2._m[4] + m13 * m2._m[5] + m23 * m2._m[6] + _m[15] * m2._m[7];
        _m[11] = m03 * m2._m[8] + m13 * m2._m[9] + m23 * m2._m[10] + _m[15] * m2._m[11];
        _m[15] = m03 * m2._m[12] + m13 * m2._m[13] + m23 * m2._m[14] + _m[15] * m2._m[15];

        return *this;
    }

    /**
     * Multiply matrices.
     * @param m2 matrix multiplier
     * @return multiplied matrices
     */
    Matrix4x4 operator*(const Matrix4x4 &m2) const
    {
        return Matrix4x4(
            _m[0] * m2._m[0] + _m[4] * m2._m[1] + _m[8] * m2._m[2] + _m[12] * m2._m[3],
            _m[0] * m2._m[4] + _m[4] * m2._m[5] + _m[8] * m2._m[6] + _m[12] * m2._m[7],
            _m[0] * m2._m[8] + _m[4] * m2._m[9] + _m[8] * m2._m[10] + _m[12] * m2._m[11],
            _m[0] * m2._m[12] + _m[4] * m2._m[13] + _m[8] * m2._m[14] + _m[12] * m2._m[15],

            _m[1] * m2._m[0] + _m[5] * m2._m[1] + _m[9] * m2._m[2] + _m[13] * m2._m[3],
            _m[1] * m2._m[4] + _m[5] * m2._m[5] + _m[9] * m2._m[6] + _m[13] * m2._m[7],
            _m[1] * m2._m[8] + _m[5] * m2._m[9] + _m[9] * m2._m[10] + _m[13] * m2._m[11],
            _m[1] * m2._m[12] + _m[5] * m2._m[13] + _m[9] * m2._m[14] + _m[13] * m2._m[15],

            _m[2] * m2._m[0] + _m[6] * m2._m[1] + _m[10] * m2._m[2] + _m[14] * m2._m[3],
            _m[2] * m2._m[4] + _m[6] * m2._m[5] + _m[10] * m2._m[6] + _m[14] * m2._m[7],
            _m[2] * m2._m[8] + _m[6] * m2._m[9] + _m[10] * m2._m[10] + _m[14] * m2._m[11],
            _m[2] * m2._m[12] + _m[6] * m2._m[13] + _m[10] * m2._m[14] + _m[14] * m2._m[15],

            _m[3] * m2._m[0] + _m[7] * m2._m[1] + _m[11] * m2._m[2] + _m[15] * m2._m[3],
            _m[3] * m2._m[4] + _m[7] * m2._m[5] + _m[11] * m2._m[6] + _m[15] * m2._m[7],
            _m[3] * m2._m[8] + _m[7] * m2._m[9] + _m[11] * m2._m[10] + _m[15] * m2._m[11],
            _m[3] * m2._m[12] + _m[7] * m2._m[13] + _m[11] * m2._m[14] + _m[15] * m2._m[15]);
    }

    /**
     * Multiply by vector.
     * @param v vector multiplier
     * @return multiplied matrix - a vector
     */
    Vector2 operator*(const Vector2 &v) const
    {
        return Vector3(
            _m[0] * v.x + _m[4] * v.y + _m[12],
            _m[1] * v.x + _m[5] * v.y + _m[13]);
    }

    /**
     * Multiply by vector.
     * @param v vector multiplier
     * @return multiplied matrix - a vector
     */
    Vector3 operator*(const Vector3 &v) const
    {
        return Vector3(
            _m[0] * v.x + _m[4] * v.y + _m[8] * v.z + _m[12],
            _m[1] * v.x + _m[5] * v.y + _m[9] * v.z + _m[13],
            _m[2] * v.x + _m[6] * v.y + _m[10] * v.z + _m[14]);
    }

    /**
     * Multiply by vector.
     * @param v vector multiplier
     * @return multiplied matrix - a vector
     */
    Vector4 operator*(const Vector4 &v) const
    {
        return Vector4(
            _m[0] * v.x + _m[4] * v.y + _m[8] * v.z + _m[12] * v.w,
            _m[1] * v.x + _m[5] * v.y + _m[9] * v.z + _m[13] * v.w,
            _m[2] * v.x + _m[6] * v.y + _m[10] * v.z + _m[14] * v.w,
            _m[3] * v.x + _m[7] * v.y + _m[11] * v.z + _m[15] * v.w);
    }

    /**
     * Multiply by scalar.
     * @param s scalar mutliplier
     * @return multiplied matrix
     */
    Matrix4x4 &operator*=(float s)
    {
        _m[0] *= s;    _m[4] *= s;    _m[8] *= s;    _m[12] *= s;
        _m[1] *= s;    _m[5] *= s;    _m[9] *= s;    _m[13] *= s;
        _m[2] *= s;    _m[6] *= s;    _m[10] *= s;    _m[14] *= s;
        _m[3] *= s;    _m[7] *= s;    _m[11] *= s;    _m[15] *= s;
        return *this;
    }

    /**
     * Multiply by scalar.
     * @param s scalar multiplier
     * @return multiplied matrix
     */
    Matrix4x4 operator*(float s) const
    {
        return Matrix4x4(
            _m[0] * s, _m[4] * s, _m[8] * s, _m[12] * s,
            _m[1] * s, _m[5] * s, _m[9] * s, _m[13] * s,
            _m[2] * s, _m[6] * s, _m[10] * s, _m[14] * s,
            _m[3] * s, _m[7] * s, _m[11] * s, _m[15] * s);
    }

    /**
     * Divide by scalar.
     * @param s scalar divisor
     * @return divided matrix
     */
    Matrix4x4 &operator/=(float s)
    {
        s = s ? 1.0f/s : 1.0f;

        _m[0] *= s;    _m[4] *= s;    _m[8] *= s;    _m[12] *= s;
        _m[1] *= s;    _m[5] *= s;    _m[9] *= s;    _m[13] *= s;
        _m[2] *= s;    _m[6] *= s;    _m[10] *= s;    _m[14] *= s;
        _m[3] *= s;    _m[7] *= s;    _m[11] *= s;    _m[15] *= s;
        return *this;
    }

    /**
     * Divide by scalar.
     * @param s scalar divisor
     * @return divided matrix
     */
    Matrix4x4 operator/(float s) const
    {
        s = s ? 1.0f/s : 1.0f;

        return Matrix4x4(
            _m[0] * s, _m[4] * s, _m[8] * s, _m[12] * s,
            _m[1] * s, _m[5] * s, _m[9] * s, _m[13] * s,
            _m[2] * s, _m[6] * s, _m[10] * s, _m[14] * s,
            _m[3] * s, _m[7] * s, _m[11] * s, _m[15] * s);
    }

    /**
     * Matrices equal.
     * @param m2 matrix to compare
     * @return true if all components equal
     */
    bool operator==(const Matrix4x4 &m2) const
    {
        return
        (
            _m[0] == m2._m[0] && _m[4] == m2._m[4] && _m[8] == m2._m[8] && _m[12] == m2._m[12] &&
            _m[1] == m2._m[1] && _m[5] == m2._m[5] && _m[9] == m2._m[9] && _m[13] == m2._m[13] &&
            _m[2] == m2._m[2] && _m[6] == m2._m[6] && _m[10] == m2._m[10] && _m[14] == m2._m[14] &&
            _m[3] == m2._m[3] && _m[7] == m2._m[7] && _m[11] == m2._m[11] && _m[15] == m2._m[15]
        );
    }

    /**
     * Matrices differ.
     * @param m2 matrix to compare
     * @return true if any of components differs
     */
    bool operator!=(const Matrix4x4 &m2) const
    {
        return
        (
            _m[0] != m2._m[0] || _m[4] != m2._m[4] || _m[8] != m2._m[8] || _m[12] != m2._m[12] ||
            _m[1] != m2._m[1] || _m[5] != m2._m[5] || _m[9] != m2._m[9] || _m[13] != m2._m[13] ||
            _m[2] != m2._m[2] || _m[6] != m2._m[6] || _m[10] != m2._m[10] || _m[14] != m2._m[14] ||
            _m[3] != m2._m[3] || _m[7] != m2._m[7] || _m[11] != m2._m[11] || _m[15] != m2._m[15]
        );
    }

    /**
     * Floats array cast.
     * @return floats array
     */
    operator float *()
    {
        return _m;
    }

    /**
     * Floats const array cast.
     * @return floats const array
     */
    operator const float *() const
    {
        return _m;
    }

    /**
     * 2x2 sub-matrix cast.
     * @return upper left 2x2 sub-matrix.
     */
    operator Matrix2x2() const
    {
        return Matrix2x2(
            _m[0],    _m[4],
            _m[1],    _m[5]);
    }

    /**
     * 3x3 sub-matrix cast.
     * @return upper left 3x3 sub-matrix.
     */
    operator Matrix3x3() const
    {
        return Matrix3x3(
            _m[0],    _m[4],    _m[8],
            _m[1],    _m[5],    _m[9],
            _m[2],    _m[6],    _m[10]);
    }

    /**
     * Quaternion cast.
     * @return quaternion created from matrix orientation.
     */
    operator Quaternion() const
    {
        float tr = _m[0] + _m[5] + _m[10];

        if(tr > 0.0f)
        {
            float s = 0.5f / sqrtf(tr + 1.0f);

            return Quaternion((_m[6] - _m[9]) * s, (_m[8] - _m[2]) * s, (_m[1] - _m[4]) * s, 0.25f / s);
        }
        else if(_m[0] > _m[5] && _m[0] > _m[10])
        {
            float s = 2.0f * sqrtf(1.0f + _m[0] - _m[5] - _m[10]);

            return Quaternion(-0.25f * s, (-_m[1] - _m[4]) / s, (-_m[2] - _m[8]) / s, (_m[9] - _m[6]) / s);
        }
        else if(_m[5] > _m[10])
        {
            float s = 2.0f * sqrtf(1.0f + _m[5] - _m[0] - _m[10]);

            return Quaternion((-_m[1] - _m[4]) / s, -0.25f * s, (-_m[6] - _m[9]) / s, (_m[2] - _m[8]) / s);
        }
        else
        {
            float s = 2.0f * sqrtf(1.0f + _m[10] - _m[0] - _m[5]);

            return Quaternion((-_m[2] - _m[8]) / s, (-_m[6] - _m[9]) / s, -0.25f * s, (_m[4] - _m[1]) / s);
        }
    }

    /**
     * Transpose matrix.
     * @return transposed matrix.
     */
    Matrix4x4 &transpose()
    {
        std::swap(_m[1], _m[4]);
        std::swap(_m[2], _m[8]);
        std::swap(_m[3], _m[12]);
        std::swap(_m[6], _m[9]);
        std::swap(_m[7], _m[13]);
        std::swap(_m[11], _m[14]);

        return *this;
    }

    /**
     * Invert matrix.
     * @return inverted matrix.
     */
    Matrix4x4 &invert()
    {
        Matrix3x3 subm = *this;
//        if(!_m[3] && !_m[7] && !_m[11] && _m[15] == 1.0f &&
//            subm * subm.transpose() == Matrix3x3())
//        {
            operator=(subm.transpose());
            Vector3 t = subm * -Vector3(m[3]);
            _m[12] = t.x; _m[13] = t.y; _m[14] = t.z;
            return *this;
//        }
//
//        // transpose matrix
//        const float *mat = _m;
//        float src[16];
//        for(int i = 0; i < 4; ++i)
//        {
//            src[i] = mat[i*4];
//            src[i + 4] = mat[i*4 + 1];
//            src[i + 8] = mat[i*4 + 2];
//            src[i + 12] = mat[i*4 + 3];
//        }
//
//        // calculate pairs for first 8 elements (cofactors)
//        float tmp[12];
//        tmp[0] = src[10] * src[15];
//        tmp[1] = src[11] * src[14];
//        tmp[2] = src[9] * src[15];
//        tmp[3] = src[11] * src[13];
//        tmp[4] = src[9] * src[14];
//        tmp[5] = src[10] * src[13];
//        tmp[6] = src[8] * src[15];
//        tmp[7] = src[11] * src[12];
//        tmp[8] = src[8] * src[14];
//        tmp[9] = src[10] * src[12];
//        tmp[10] = src[8] * src[13];
//        tmp[11] = src[9] * src[12];
//
//        // calculate first 8 elements (cofactors)
//        float *dst = (float *)(this);
//
//        dst[0] = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7];
//        dst[0] -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7];
//        dst[1] = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7];
//        dst[1] -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7];
//        dst[2] = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7];
//        dst[2] -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7];
//        dst[3] = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6];
//        dst[3] -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6];
//        dst[4] = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3];
//        dst[4] -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3];
//        dst[5] = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3];
//        dst[5] -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3];
//        dst[6] = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3];
//        dst[6] -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3];
//        dst[7] = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2];
//        dst[7] -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2];
//
//        // calculate pairs for second 8 elements (cofactors)
//        tmp[0] = src[2]*src[7];
//        tmp[1] = src[3]*src[6];
//        tmp[2] = src[1]*src[7];
//        tmp[3] = src[3]*src[5];
//        tmp[4] = src[1]*src[6];
//        tmp[5] = src[2]*src[5];
//
//        tmp[6] = src[0]*src[7];
//        tmp[7] = src[3]*src[4];
//        tmp[8] = src[0]*src[6];
//        tmp[9] = src[2]*src[4];
//        tmp[10] = src[0]*src[5];
//        tmp[11] = src[1]*src[4];
//
//        // calculate second 8 elements (cofactors)
//        dst[8] = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15];
//        dst[8] -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15];
//        dst[9] = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15];
//        dst[9] -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15];
//        dst[10] = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15];
//        dst[10]-= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15];
//        dst[11] = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14];
//        dst[11]-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14];
//        dst[12] = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9];
//        dst[12]-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10];
//        dst[13] = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10];
//        dst[13]-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8];
//        dst[14] = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8];
//        dst[14]-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9];
//        dst[15] = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9];
//        dst[15]-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8];
//
//        // calculate determinant
//        float det = src[0]*dst[0] + src[1]*dst[1] + src[2]*dst[2] + src[3]*dst[3];
//
//        // calculate matrix inverse
//        det = 1.0f / det;
//        for(int i = 0; i < 16; i++)
//        {
//            dst[i] *= det;
//        }
//
//        return *this;
    }

    /**
     * Spherical linear interpolation between two matrices.
     * @param m1 first matrix
     * @param m2 second matrix
     * @param t interpolation factor.
     * @return spherically linearly interpolated matrix between m1 and m2.
     */
    friend Matrix4x4 slerp(const Matrix4x4 &m1, const Matrix4x4 &m2, float t)
    {
        return Matrix4x4(slerp(Vector4(m1.m[0]), Vector4(m2.m[0]), t),
                         slerp(Vector4(m1.m[1]), Vector4(m2.m[1]), t),
                         slerp(Vector4(m1.m[2]), Vector4(m2.m[2]), t),
                         lerp(Vector4(m1.m[3]), Vector4(m2.m[3]), t));
    }
};

}

#endif // MATRIX4X4_H_
